/*
 * Copyright (C) 2017 - 2019, AEye, Inc.
 * All rights reserved.
 */
#include "spdlog/spdlog.h"

#include "SocketCloudReader.hpp"
#include "AEyeExceptions.hpp"
#include "Utils.hpp"

#ifdef _MSC_VER
//Gavin: Necessary for M_PI to function properly
#define _USE_MATH_DEFINES // for C
#include <math.h>
#else
#include <cmath>
#endif

namespace {
constexpr float radians_to_degrees_conversion_factor = 180.F / M_PI;
}

namespace aeye {

SocketCloudReader::SocketCloudReader(const std::string &sensorAddr, uint16_t clientPort, bool udpBroadcastMode) :
    reader_(sensorAddr, clientPort, DEFAULT_SERVER_POINTS_PORT, udpBroadcastMode), pcap_queue_manager_(isRunning, maxQueueSize),
    frame_queue_manager_(isRunning, maxQueueSize), sensorAddress(sensorAddr) {
    spdlog::trace("SCR ctor called.");
}

SocketCloudReader::~SocketCloudReader() noexcept {
  spdlog::trace("SCR dtor called.");
  if (isRunning) stop();
}

void SocketCloudReader::run() {
  spdlog::trace("Started running socketCloudReader.");
  set_thread_name(AEYE_LIT("CloudRun"));
  isRunning = true;

  // UDP datagram buffer
  auto buf = std::make_unique<uint8_t[]>(UDP_BUF_LEN);

  // Datagram source
  sockaddr_in src_addr{};

  constexpr unsigned readDelayS = 0;
  constexpr unsigned readDelayuS = (30 * 1000);   // 30ms

  currentlyAccumulatingFrame.reset();

  pcap_queue_manager_.clean();
  frame_queue_manager_.clean();

  while (isRunning.load()) {
    if (pcapModeOn) {
      std::vector<uint8_t> pcapPacket = pcap_queue_manager_.pop();
      bool isFrameReady = processPacket(pcapPacket.data(), pcapPacket.size());
      if (!isFrameReady) continue;
      if (!frame_queue_manager_.wait_to_push()) {
        return;
      }
      frame_queue_manager_.push(std::move(currPointCloudFrame));
      continue;
    }

    fd_set readFlags{};
    if (!reader_.run_select(readDelayS, readDelayuS, readFlags)) {
      continue;
    }

    if (!reader_.is_set(readFlags) || !isRunning) {
      // Timeout or wrong checksum
      continue;
    }

    const size_t bytesReceived = reader_.receive_bytes(buf.get(), src_addr);

    if (bytesReceived == 0) {
      continue;
    }

    if (!reader_.validate_ip_addr(src_addr)) {
      continue;
    }
    processPacket(buf.get(), bytesReceived);
  }
}

void SocketCloudReader::pushRangingFrameToQueue(uint32_t expectedNumberOfPointsInFrame, Pose sensor_pose, const std::string& shotlist_name) {
  const uint32_t completedFrameId = currentlyAccumulatingFrame.getFrameId();

  const AEyePoint *points = currentlyAccumulatingFrame.getPoints();
  const auto total_num_points = currentlyAccumulatingFrame.getTotalNumPoints();
  // vector's iterator ctor - no point in pre-allocating as class member because we need to move from it into PCF
  std::vector<AEyePoint> framePoints{points, points + total_num_points};

  currPointCloudFrame = PointCloudFrame(sensorAddress, currentlyAccumulatingFrame.getFrameId(), currentlyAccumulatingFrame.getFlexFormatDataFlags(),
                                        std::move(framePoints), expectedNumberOfPointsInFrame, currentlyAccumulatingFrame.getTimeSec(), currentlyAccumulatingFrame.getTimeuSec(),
                                        sensor_pose, shotlist_name);

  numPointsInCurrentlyAccumulatingFrame = currPointCloudFrame.getNumReceivedPoints();

  // Throw a message if there's a mismatch in the number of points received vs the number of points in the delimiter
  if (expectedNumberOfPointsInFrame != numPointsInCurrentlyAccumulatingFrame) {
    spdlog::warn("Never totally completed ranging frame: {}. Expected {} points, received {}.",
                 completedFrameId, expectedNumberOfPointsInFrame, numPointsInCurrentlyAccumulatingFrame);
  }

  if (!pcapModeOn) frame_queue_manager_.push(std::move(currPointCloudFrame));
}

bool SocketCloudReader::processPacket(const uint8_t* data, size_t len) {
  bool result = false;
  if (len < sizeof(PacketHeader)) {
    return result;
  }

  // Received header
  const auto pHeader = reinterpret_cast<const PacketHeader*>(data);
  const PacketHeader& header  = *pHeader;

  // Check marker to see if it's AEYE, i.e., if it's something sent from our sensor
  if (!Reader::find_marker(header.marker)) {
    spdlog::warn("Encountered invalid header in SCR.");
    return result;
  }

  size_t dataRead = sizeof(PacketHeader);

  // Packet data
  const auto packetType = static_cast<AEyeCommands>(header.type);

  if (packetType == AEyeCommands::RET_POINT_CLOUD) {

    // Do a version check
    if (header.version!=RET_POINTS_VERSION) {
      spdlog::warn("Unsupported version of Point Cloud packet received. Expecting version {}.", RET_POINTS_VERSION);
      return false;
    }

    const auto pPointReturnsBody = reinterpret_cast<const PointReturnsBody*>(data + dataRead);

    const uint16_t mask = pPointReturnsBody->pointReturnMask;
    const size_t pointSize =   sizeof(PointReturnSphericalCoords) * (mask & PointReturnFlags::SPHERICAL_COORDINATES ? 1 : 0)
                             + sizeof(PointReturnUnused_2)        * (mask & PointReturnFlags::UNUSED_2 ? 1 : 0)
                             + sizeof(PointReturnIntensity)       * (mask & PointReturnFlags::INTENSITY ? 1 : 0)
                             + sizeof(PointReturnUnused_8)        * (mask & PointReturnFlags::UNUSED_8 ? 1 : 0)
                             + sizeof(PointReturnUnused_6)        * (mask & PointReturnFlags::UNUSED_6 ? 1 : 0)
                             + sizeof(PointReturnUnused_7)        * (mask & PointReturnFlags::UNUSED_7 ? 1 : 0)
                             + sizeof(PointReturnType)            * (mask & PointReturnFlags::UNUSED_7 ? 0 : 1)
                             + sizeof(PointReturnUnused_1)        * (mask & PointReturnFlags::UNUSED_1 ? 1 : 0)
                             + sizeof(PointReturnUnused_5)        * (mask & PointReturnFlags::UNUSED_5 ? 1 : 0)
                             ;
    if (0u == pointSize) { // Dodge division by zero
      spdlog::warn("Received PRM: {}, thus point size: {}", mask, pointSize);
      return result;
    }
    const size_t numPoints = (header.bodySize - sizeof(PointReturnsBody)) / pointSize;
    const size_t expectedPacketSize = dataRead + sizeof(PointReturnsBody) + numPoints * pointSize;
    if (len >= expectedPacketSize) {
      if (len != expectedPacketSize) {
        spdlog::warn("Wrong UDP packet size. Read {} bytes from socket, expected {}.", len, expectedPacketSize);
      }
      const PointReturnsBody& pointReturnsBody = *pPointReturnsBody;
      dataRead += sizeof(PointReturnsBody);
      result = processPointCloudPacketPayload(header, pointReturnsBody, data + dataRead, numPoints);
    }
    else {
      spdlog::warn("Wrong UDP packet size. Read {} bytes from socket, expected {}.", len, expectedPacketSize);
      return result;
    }
  }
  else if (packetType == AEyeCommands::RET_POINT_CLOUD_DELIM) {
    const auto packetDelimiter = reinterpret_cast<const PacketPointCloudDelimiter*>(data);

    // Do a version check
    if (header.version > RET_POINT_CLOUD_DELIM_VERSION) {
      spdlog::warn("Unsupported version of Point Cloud delimiter packet received. Expecting version {}.", RET_POINT_CLOUD_DELIM_VERSION);
      return false;
    }

    const uint32_t frameId = packetDelimiter->frameId;
    const uint32_t expectedNumberOfPointsInFrame = packetDelimiter->numPointsTotalInFrame;

    Pose sensor_pose{};
    std::string shotlist_name;
    if (header.version == RET_POINT_CLOUD_DELIM_VERSION) {
      sensor_pose = packetDelimiter->sensorPose; // Sensor pose angles are received in radians, need to convert to degrees
      sensor_pose.pitch *= radians_to_degrees_conversion_factor;
      sensor_pose.yaw *= radians_to_degrees_conversion_factor;
      sensor_pose.roll *= radians_to_degrees_conversion_factor;
      shotlist_name = packetDelimiter->scanPatternName;
    }

    // If match, then go ahead and push to the queue
    if (frameId == currentlyAccumulatingFrame.getFrameId()) {

      frameCompleted = true;
      result = frameCompleted;
      const uint32_t completedFrameId = currentlyAccumulatingFrame.getFrameId();

      // Push the current frame to the stack
      pushRangingFrameToQueue(expectedNumberOfPointsInFrame, sensor_pose, shotlist_name);

      // Start accumulating points in the next reserved frame
      currentlyAccumulatingFrame.reset();
      currentlyAccumulatingFrame.setTime(header.tsSec, header.tsuSec);
      currentlyAccumulatingFrame.setFrameId(completedFrameId+1);
    }
  }
  else {
    // Ignore mystery packet. Should never happen!
    spdlog::error("Type not valid: {}.", packetType);
  }
  return result;
}

bool SocketCloudReader::processPointCloudPacketPayload(const PacketHeader& packetHeader,
                                                       const PointReturnsBody& packetBody,
                                                       const uint8_t* pData, size_t numPoints) {

  // Get frame ID of point cloud frame we're currently constructing
  const uint32_t currentlyAccumulatingFrameID = currentlyAccumulatingFrame.getFrameId();

  // See if the frameID of the received packet matches
  const uint32_t newReceivedFrameId = packetBody.frameId;

  // If they match, add the points to rangeFrameData1
  if (newReceivedFrameId == currentlyAccumulatingFrameID) {
    currentlyAccumulatingFrame.addPoints(packetBody, pData, numPoints);
  }

  // If the frameID of the received packet is lower than the currently accumulating frame, then throw it away
  else if (newReceivedFrameId < currentlyAccumulatingFrameID) {
    return false;
  }

  // If not, throw a warning and create a new frame
  else {

    bool result = false;

    // Taking into account that currentlyAccumulatingFrame is always initialized to 0,
    // if the first received frame has a frame id bigger than 0, an empty frame with frameId=0 would be created here.
    // Thus, it is important to discard this initial case
    if (!(currentlyAccumulatingFrame.getTotalNumPoints() == 0 && currentlyAccumulatingFrame.getFrameId() == 0)) {

      // If we got to this point, the delimiter packet wasn't received on time, so throw a warning message
      spdlog::warn("Did not receive delimiter for ranging frame: {}.", currentlyAccumulatingFrame.getFrameId());

      // Push the current frame to the stack
      Pose empty_pose{};
      pushRangingFrameToQueue(0, empty_pose, "");

      result = true;
    }

    // Start accumulating points in the next reserved frame
    currentlyAccumulatingFrame.reset();
    currentlyAccumulatingFrame.setTime(packetHeader.tsSec, packetHeader.tsuSec);
    currentlyAccumulatingFrame.addPoints(packetBody, pData, numPoints);
    currentlyAccumulatingFrame.setFrameId(packetBody.frameId);

    return result;
  }
  return false;
}

PointCloudFrame SocketCloudReader::pop() {
  return frame_queue_manager_.pop();
}

void SocketCloudReader::pushPcapPacket(std::vector<uint8_t> &&data) {
  pcap_queue_manager_.push(std::move(data));
}

void SocketCloudReader::start() {
  reader_.start();
}

void SocketCloudReader::stop() noexcept {
  if (!isRunning) return;
  isRunning = false;
  reader_.stop();
  spdlog::trace("Stopped SCR.");
}

void SocketCloudReader::setPcapMode(bool value) noexcept {
  pcapModeOn = value;
}

void SocketCloudReader::resetReader() {
  if (pcapModeOn) {
    pcap_queue_manager_.clean();
    frame_queue_manager_.clean();
    currentlyAccumulatingFrame.reset();
  }
}
} // end namespace aeye
